﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;
using SpaceMan.Entities;

namespace SpaceMan.Physics
{
    public class PHY_Base
    {
        public physicsStateDerivative derivativeState;
        public physicsState currentState;
        public long mass;
        Vector2 forces, confinementXY, originBB;
        Vector2 gravityForce;
        public bool isStatic, MarkedForDeletion;
        public List<PHY_Collider> Collider;
        public PHY_GravityWell GravityWell;
        bool locked;
        bool confined;
        int confinedWidth, confinedHeight;
        public Rectangle BoundingBox;
        public PHY_Base parent;
        public List<PHY_Base> children;
        Vector2 origin, parentSpeed;
        public Vector2 OffsetFromParent, TotalSpeed;
        public I_Entity OwnerEntity;
        public Action<PHY_Base, string, bool> ParentMe;
        float drag;
        public PHY_Base(Vector2 position, long mass,float drag ,bool isStatic,List<PHY_Collider> Collider, PHY_GravityWell GravityWell, string ownerName)
        {

            this.drag = drag;
            origin = Vector2.Zero;
            children = new List<PHY_Base>();
            this.mass = mass;
            currentState.Pos = position;
            this.GravityWell = GravityWell;
            this.Collider = Collider;
            this.GravityWell = GravityWell;
            this.Collider = Collider;

            this.isStatic = isStatic;
            Vector2 minimum = Vector2.Zero;
            Vector2 maximum = Vector2.Zero;
            if (Collider != null)
            {
                foreach (PHY_Collider collider in Collider)
                {
                    if (collider.Rectangle.Right > maximum.X)
                    {
                        maximum.X = collider.Rectangle.Right;
                    }
                    else if (collider.Rectangle.Bottom > maximum.Y)
                    {
                        maximum.Y = collider.Rectangle.Bottom;
                    }
                }
            }

            minimum = maximum;

            if (Collider != null)
            {
                foreach (PHY_Collider collider in Collider)
                {
                    if (collider.Rectangle.X < minimum.X)
                    {
                        minimum.X = collider.Rectangle.X;
                    }
                    else if (collider.Rectangle.Y < minimum.Y)
                    {
                        minimum.Y = collider.Rectangle.Y;
                    }
                }
            }
            originBB = minimum;
            BoundingBox = new Rectangle((int)minimum.X, (int)minimum.Y, (int)maximum.X, (int)maximum.Y);
            S_Global.AddBody(this, ownerName);
        }

        public void Initialize()
        {
        }

        public void Update()
        {
            float dt = S_Global.getTimescale() / 1000;

            if (parent != null)
            {
                origin = parent.currentState.Pos;
                OffsetFromParent = currentState.Pos - parent.currentState.Pos;
                parentSpeed = parent.currentState.V;
                
                
            }


            if (!locked)
            {
                currentState.V += forces;
                currentState.V *= MathHelper.Clamp(drag + ((1 - drag) * (1 - S_Global.TIMESCALE)), 0, 1);
                currentState.Pos += (currentState.V + parentSpeed) * dt;
                TotalSpeed = currentState.V + parentSpeed;
                forces = Vector2.Zero;
            }

            //if (children != null)
            //{
            //    foreach (PHY_Base Child in children)
            //    {
            //        Child.currentState.Pos = currentState.Pos + Child.OffsetFromParent;
            //    }
            //}

            if (confined)
            {
                Vector2 _checkdif = currentState.Pos;
                currentState.Pos.X = MathHelper.Clamp(currentState.Pos.X, confinementXY.X, confinedWidth - BoundingBox.Width);
                currentState.Pos.Y = MathHelper.Clamp(currentState.Pos.Y, confinementXY.Y, confinedHeight - BoundingBox.Height);

                if (_checkdif.X != currentState.Pos.X)
                {
                    currentState.V.X = 0;
                }

                if (_checkdif.Y != currentState.Pos.Y)
                {
                    currentState.V.Y = 0;
                }

            }

            

            BoundingBox.Y = (int)(currentState.Pos.Y + originBB.Y + origin.Y);
            BoundingBox.X = (int)(currentState.Pos.X + originBB.X + origin.X);

            if (GravityWell != null)
            {
                GravityWell.Update(currentState.Pos);
                
            }

            if (Collider != null)
            {
                foreach (PHY_Collider collider in Collider)
                {
                    collider.Update(currentState.Pos + origin);
                }
            }

            //integrate(currentState, 0, S_Global.getTimescale());



        }

        public void Confine(Vector2 XY, int MaxRight, int MaxDown)
        {
            confinementXY = XY;
            confinedWidth = MaxRight + (int)confinementXY.X;
            confinedHeight = MaxDown + (int)confinementXY.Y;
            confined = true;
        }

        public void Parent(PHY_Base Parent, bool LockPosition)
        {
            parent = Parent;
            Parent.children.Add(this);
            locked = LockPosition;
        }

        void ChangePos(Vector2 Position)
        {
            currentState.Pos = Position;
        }

        public void Addforce(Vector2 direction)
        {
            forces += direction / mass;
        }

        public void AddGravity(Vector2 direction)
        {
            gravityForce += direction;
        }

        physicsStateDerivative evaluate(physicsState initial, float t, float dt, physicsStateDerivative d)
        {
            
            physicsState state;
            state.Pos = initial.Pos + d.DX * dt;
            state.V = initial.V + d.DV * dt;

            physicsStateDerivative output;
            output.DX = state.V;

            output.DV = acceleration(d, t+dt);
            return output;
        }

        Vector2 acceleration(physicsStateDerivative state, float t)
        {
            //const float k = 10f;
            //const float b = 1f;
            //return -k * state.DX- b * state.DV;

            return currentState.Pos * t * (forces + gravityForce);
        }

        void integrate(physicsState state, float t, float dt)
        {
            dt /= 1000;
            physicsStateDerivative a = evaluate(state, t, 0.0f, derivativeState);
            physicsStateDerivative b = evaluate(state, t, dt*0.5f, a);
            physicsStateDerivative c = evaluate(state, t, dt*0.5f, b);
            physicsStateDerivative d = evaluate(state, t, dt, c);

            Vector2 dxdt = 1.0f/6.0f * (a.DX + 2.0f*(b.DX + c.DX) + d.DX);
            Vector2 dvdt = 1.0f / 6.0f * (a.DX + 2.0f * (b.DV + c.DV) + d.DV);

            currentState.Pos = state.Pos + dxdt * dt;
            currentState.V = state.V + dvdt * dt;
        }
    }
}
